#include <stdio.h>
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include <iostream>
#include <boost/foreach.hpp>
#include "std_msgs/Int32.h"
#include "distances/DistancesMatrix.h"
#include "pi_tracker/Skeleton.h"


int main()
{
rosbag::Bag bag("/home/dalton/bagfiles/distanciasSimples2.bag");
rosbag::View view(bag, rosbag::TopicQuery("/matrix"));

FILE * pFile;

   pFile = fopen ("distances2.txt","w");


BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
    distances::DistancesMatrix::ConstPtr msg = m.instantiate<distances::DistancesMatrix>();
    if (msg != NULL)
      for(int i=0;i<15;i++){

	for(int j=0; j<15;j++){
 
	fprintf (pFile, "%f ",msg->dM[i].v[j]);
	}
	
	fprintf (pFile, "\n");
      }
      std::cout << std::endl;
    
    
    
    
}
fclose (pFile);
bag.close(); 
}